/*!
  @file parameter_adjuster.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/JointState.h>

#include <boost/thread.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

namespace mrover
{

/*! 
 *  @brief  An awesome class
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  This is a detailed description of the class 
 */

class ParameterAdjuster 
{
 public:
  ParameterAdjuster(ros::NodeHandle &nh) 
      : nh_(nh)
      , private_nh_("~")
      , winname_("Parameters")
      , tilt_p_(30)
      , height_p_(90)
      , baseline_p_(30)
      , sigma_p_(30)
      , speckle_p_(10)
      , dummy_msg(new std_msgs::Int32)
  {
    // initializing pub/sub, server
    sigma_pub_ = nh_.advertise<std_msgs::Float32>(nh_.resolveName("mapping/obj_sigma"), 5);
    speckle_pub_ = nh_.advertise<std_msgs::Float32>(nh_.resolveName("mapping/speckle_size"), 5);
    reset_pub_ = nh_.advertise<std_msgs::Int32>(nh_.resolveName("reset_out"), 5);
    tilt_pub_ = nh_.advertise<sensor_msgs::JointState>(nh_.resolveName("/rover/joint_states"), 5);

    // initializing optional parameters
    // e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);


    cv::namedWindow(winname_);
    cv::createTrackbar("tilt[cm]", winname_, &tilt_p_, 90);
    //cv::createTrackbar("camera_height[cm]", winname_, &height_p_, 200);
    //cv::createTrackbar("baseline[cm]", winname_, &baseline_p_, 50);
    //cv::createTrackbar("obj_detect/sigma(x0.1)", winname_, &sigma_p_, 50);
    //cv::createTrackbar("obj_detect/speckle_size[pixel]", winname_, &speckle_p_, 80);
    cv::waitKey(1);

    boost::thread thread_tf(&ParameterAdjuster::tfBroadcast, this);
    boost::thread thread_param(&ParameterAdjuster::publishParams, this);
    boost::thread thread_window(&ParameterAdjuster::refleshWindow, this);

    cv::Mat m = cv::Mat::zeros(1, 480, CV_8U);
    cv::imshow(winname_, m);
    cv::waitKey(1);
  }

  ~ParameterAdjuster() { }

 private:
  double deg2rad(double deg) { return deg / 180 * M_PI; }
  void tfBroadcast();
  void publishParams();
  void refleshWindow();

  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;
  tf::TransformBroadcaster tf_pub_; 
  tf::Transform transform_;
  ros::Publisher sigma_pub_;
  ros::Publisher speckle_pub_;
  ros::Publisher reset_pub_;
  ros::Publisher tilt_pub_;

  std::string winname_;
  int tilt_p_;
  int height_p_;
  int baseline_p_;
  int sigma_p_;
  int speckle_p_;
  std_msgs::Int32Ptr dummy_msg;

};

void ParameterAdjuster::tfBroadcast()
{
  ros::Rate r(10);
  while (nh_.ok())
  {
    /*
    tf::Vector3 p(0, 0.01*baseline_p_/2, 0.01*height_p_);
    tf::Quaternion q;
    q.setRPY(deg2rad(-90 - tilt_p_), 0, deg2rad(-90));
    transform_.setOrigin(p);
    transform_.setRotation(q);
    tf_pub_.sendTransform(tf::StampedTransform(transform_, ros::Time::now(), "/rover/base_link", "/rover/left_camera"));
    */
    tf::Vector3 p(0, 0, 0);
    tf::Quaternion q;
    q.setRPY(0, deg2rad(tilt_p_), 0);
    transform_.setOrigin(p);
    transform_.setRotation(q);
    tf_pub_.sendTransform(tf::StampedTransform(transform_, ros::Time::now(), "/rover/ginbar_base", "/rover/ginbar"));

    sensor_msgs::JointStatePtr msg(new sensor_msgs::JointState);
    msg->header.stamp = ros::Time::now();
    msg->name.push_back("/rover/tilt");
    msg->position.push_back(deg2rad(tilt_p_));
    //tilt_pub_.publish(msg);
    ros::spinOnce();
    r.sleep();
  }
}

void ParameterAdjuster::publishParams()
{
  ros::Rate r(0.5);
  while (nh_.ok())
  {
    std_msgs::Float32 sigma_msg;
    sigma_msg.data = sigma_p_ * 0.1;
    sigma_pub_.publish(sigma_msg);
    sigma_pub_.publish(sigma_msg);

    std_msgs::Float32 speckle_msg;
    speckle_msg.data = speckle_p_;
    speckle_pub_.publish(speckle_msg);
    ros::spinOnce();

    r.sleep();
  }
}

void ParameterAdjuster::refleshWindow()
{
  while (nh_.ok())
  {
    char c = cv::waitKey(3);
    if (c == 'r')
      reset_pub_.publish(dummy_msg);
  }
}

}  // namespace mrover

int main(int ac, char **av)
{
  ros::init(ac, av, "ParameterAdjuster");
  ros::NodeHandle nh;
  mrover::ParameterAdjuster handler(nh);
  ros::spin();
  return 0;
}

